package moro;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:  The Controller class controls the robot
 * Copyright:    Copyright (c) 2002
 * Company:      Universit di Bergamo
 * @author       Davide Brugali, Bleker, van Zutphen, Stellingwerf, Grobben
 * @version 1.0
 */
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PipedInputStream;
import java.io.PipedOutputStream;
import java.io.PrintWriter;
import java.util.Stack;
import java.util.StringTokenizer;

public class Controller implements Runnable {

    Robot robot = null;
    OccupancyMap map = new OccupancyMap();
    String result = "";
    double position[] = new double[3];
    double measures[] = new double[360];
    double measuresSonar[] = new double[360];
    int left = 0;
    int right = 0;
    boolean haltRequested = false;
    boolean explorationComplete = false;
    int movedForward = 0;
    private int draws = 0;
    MapCalculator mapCalculator;
    boolean useSonar;
    private int defaultDistance = map.getCellDim();

    public Controller(Robot robot) {
        this.robot = robot;
        if (robot == null) {
            System.exit(1);
        }
    }

    public void start(boolean useSonar) {
        robot.start();
        this.useSonar = useSonar;
        // This will cause the controller run() to be started in a separate thread.
        new Thread(this).start();
    }

    // quit methode aangeroepen vanuit SimulatorWindow
    public void quit() {
        haltRequested = true;
    }

    // continue methode aangeroepen vanuit SimulatorWindow
    public void continueTask() {
        haltRequested = false;
    }

    /**
     * In this method the controller sends commands to the robot and its devices.
     * At the moment all the commands are hardcoded. 
     * The exercise is to let the controller make intelligent decisions based on 
     * what has been discovered so far. This information is contained in the OccupancyMap.
     *  Robot Commands
     *  R1.GETPOS         = get actual position
     *  ---------------
     *  L1.SCAN           = measure distance with Laser
     *  L1.READ           = measure distance to nearest obstacle in front laser
     *  L1.DETECT         = scan -45 / +45 angle in front robot, repeat until obstacle detection
     *  ---------------
     *  S1.SCAN           = measure distance with Sonar
     *  S1.READ           = measure distance to nearest obstacle in front sonar
     *  S1.DETECT         = scan -45 / +45 angle in front robot, repeat until obstacle detection
     *  ---------------
     *  P1.MOVEFW ##      = move forward ## centimeters
     *  P1.MOVEBW ##      = move backward ## centimeters
     *  P1.ROTATELEFT ##  = rotate left ## degrees
     *  P1.ROTATERIGHT ## = rotate right ## degrees
     * 
     */
    @Override
    public void run() {
        // calculaties in Occupancy
        mapCalculator = new MapCalculator(map, robot);

        try {
            // *create the pipe and install buffered reader/writer
            // *so we can use readLine() and println
            PipedInputStream pipeIn = new PipedInputStream();
            BufferedReader input = new BufferedReader(new InputStreamReader(pipeIn));
            PrintWriter output = new PrintWriter(new PipedOutputStream(pipeIn), true);

            // *inform robot/device in other thread where to write
            robot.setOutput(output);
            System.out.println("Controller started");

            //Rij robot naar de eerste muur en draai dan naar links
            updateRobotPosition(input);
            draw(input);

            while (moveForward(input)) {
            };

            if (turnRight(input)) {
            }
            moveForward(input);

            // -------- main loop -------------
            while ((!explorationComplete)) {


                if (turnLeft(input)) {
//                moveForward(input);
                } else if (mapCalculator.getDistanceLeft(position)
                        == (2 * defaultDistance)
                        && mapCalculator.getDistanceFront(position) > defaultDistance) {
                    moveCloserLeft(input);

                } else if (moveForward(input)) {
                } else if (turnRight(input)) {
                } else if (mapCalculator.getDistanceFront(position)
                        == defaultDistance
                        && mapCalculator.getDistanceLeft(position) == defaultDistance
                        && mapCalculator.getDistanceRight(position) == defaultDistance) {
                    moveBackwards(input);
                } else {
                    turnAround(input);
                }
                // boodschap naar Occupancy map: ready


                if (mapCalculator.occupancyMapIsExplored()
                        || mapCalculator.occupancyMapHasOuterBoundary()) {
                    map.setMapIsExplored(true);
                    map.repaint();
                    explorationComplete = true;
                    try {
                        // sleep 3 seconds
                        Thread.currentThread().sleep(3000);
                        //do what you want to do after sleeping
                    } catch (InterruptedException ie) {
                        //If this thread was interrupted by another thread 
                    }
                }
                // pauzeer robot

                while (haltRequested) {
                    try {
                        // 
                        Thread.currentThread().sleep(1000);
                        //do what you want to do after sleeping
                    } catch (InterruptedException ie) {
                        //If this thread was interrupted by another thread
                    }
                }
            }
        } catch (IOException ioe) {
        }
    }

    /**
     * Beweeg naar Voren
     * @param input
     * @return true als er naar voren kon worden bewogen
     */
    private boolean moveForward(BufferedReader input) {
        //Reset left en right als het gelukt is om twee keer naar voren te gaan
        //voorkomt raar gedrag in normale bochten

        if ((int) mapCalculator.getDistanceFront(position) > 2 * robot.getMarge()
                && (int) mapCalculator.getDistanceLeft(position) == robot.getMarge() && mapCalculator.checkWall(position)) {

            while ((int) mapCalculator.getDistanceLeft(position) == robot.getMarge()
                    && mapCalculator.getDistanceFront(position) > 2 * robot.getMarge() && mapCalculator.checkWall(position)) {
                robot.sendCommand("P1.MOVEFW " + defaultDistance);
                try {
                    result = input.readLine();
                } catch (IOException ex) {
                }
                updateRobotPosition(input);
                movedForward++;
            }

            if (movedForward > 2) {
                left = 0;
                right = 0;
                movedForward = 0;
            }

            if (mapCalculator.isScanNecessary(position)) {
                draw(input);
            }

            return true;
        } else if ((int) mapCalculator.getDistanceFront(position) > robot.getMarge()) {
            robot.sendCommand("P1.MOVEFW " + defaultDistance);
            try {
                result = input.readLine();
            } catch (IOException ex) {
            }
            updateRobotPosition(input);


            if (mapCalculator.isScanNecessary(position)) {
                draw(input);
            }
            movedForward++;

            if (movedForward > 2) {
                left = 0;
                right = 0;
                movedForward = 0;
            }

            return true;
        }

        return false;
    }

    /**
     * Draai naar Links met 90 graden draai
     * @param input
     * @return true als er naar links kon worden gedraaid 
     */
    private boolean turnLeft(BufferedReader input) {
        if (left >= 4) {
            turnRight(input);
//            left = 0;
        } else if ((int) mapCalculator.getDistanceLeft(position) > robot.getMarge() + defaultDistance) {
            try {
                robot.sendCommand("P1.ROTATELEFT " + "45");
                result = input.readLine();
                updateRobotPosition(input);

                robot.sendCommand("P1.MOVEFW " + Math.sqrt(2) * 10);
                result = input.readLine();
                updateRobotPosition(input);

                robot.sendCommand("P1.ROTATELEFT " + "45");
                result = input.readLine();
                updateRobotPosition(input);
//
//                moveForward(input);

                left++;
                right = 0;
                movedForward = 0;

            } catch (IOException ex) {
            }
            return true;
        }else if ((int) mapCalculator.getDistanceLeft90(position) > robot.getMarge() + defaultDistance) {
            try {
                robot.sendCommand("P1.ROTATELEFT " + "90");
                result = input.readLine();
                updateRobotPosition(input);

                moveForward(input);

                left++;
                right = 0;
                movedForward = 0;

            } catch (IOException ex) {
            }
            return true;
        }
        return false;
    }

    /**
     * Draai naar Links mbv 2x 45 graden draai  
     * @param input
     * @return true als er naar links kon worden gedraaid 
     */
    private boolean moveCloserLeft(BufferedReader input) {

        try {
            robot.sendCommand("P1.ROTATELEFT " + "45");
            result = input.readLine();
            updateRobotPosition(input);

            robot.sendCommand("P1.MOVEFW " + (defaultDistance * Math.sqrt(2)));
            result = input.readLine();
            updateRobotPosition(input);

            robot.sendCommand("P1.ROTATERIGHT " + "45");
            result = input.readLine();
            updateRobotPosition(input);

            return true;
        } catch (IOException ex) {
        }
        return false;
    }

    /**
     * Draai naar Rechts mbv 2x 45 graden draai
     * @param input
     * @return true als er naar rechts kon worden gedraaid 
     */
    private boolean moveCloserRight(BufferedReader input) {

        try {

            robot.sendCommand("P1.ROTATERIGHT " + "45");
            result = input.readLine();
            updateRobotPosition(input);

            robot.sendCommand("P1.MOVEFW " + (defaultDistance * Math.sqrt(2)));
            result = input.readLine();
            updateRobotPosition(input);

            robot.sendCommand("P1.ROTATELEFT " + "45");
            result = input.readLine();
            updateRobotPosition(input);

            return true;
        } catch (IOException ex) {
        }
        return false;
    }

    /**
     * Draai naar Rechts mbv van 90 graden draai
     * of 2 x 45 graden indien mogelijk en genoeg ruimte
     * @param input
     * @return true als er naar rechts kon worden gedraaid 
     */
    private boolean turnRight(BufferedReader input) {

        if (right >= 4) {
//            right = 0;
            turnLeft(input);

        } else if ((int) mapCalculator.getDistanceRight(position) > robot.getMarge()) {
            try {

                if ((int) mapCalculator.getDistanceRight(position) == (2 * defaultDistance)) {
                    System.out.println("rechts: " + mapCalculator.getDistanceRight(position));
                    robot.sendCommand("P1.ROTATERIGHT " + "90");
                    result = input.readLine();
                    updateRobotPosition(input);
                } else {
                    robot.sendCommand("P1.ROTATERIGHT " + "45");
                    result = input.readLine();
                    updateRobotPosition(input);

                    robot.sendCommand("P1.MOVEFW " + (defaultDistance * Math.sqrt(2)));
                    result = input.readLine();
                    updateRobotPosition(input);

                    robot.sendCommand("P1.ROTATERIGHT " + "45");
                    result = input.readLine();
                    updateRobotPosition(input);
//                moveForward(input);
                }

                right++;
                left = 0;
                movedForward = 0;


                updateRobotPosition(input);
                return true;

            } catch (IOException ex) {
            }
        }
        return false;
    }

    /**
     * Draai Robot 180 graden Rechtsom of Linksom
     * dit afhankelijk van de beschikbare ruimte
     * @param input
     * @return true als er 180 graden gedraaid kon worden 
     */
    private boolean turnAround(BufferedReader input) {
        try {
            if (mapCalculator.getDistanceRight(position) > defaultDistance) {
                robot.sendCommand("P1.ROTATERIGHT " + "180");
                result = input.readLine();
                updateRobotPosition(input);
                moveForward(input);
            } else {
                robot.sendCommand("P1.ROTATELEFT " + "180");
                result = input.readLine();
                updateRobotPosition(input);
                moveForward(input);
            }

        } catch (IOException ex) {
        }
        updateRobotPosition(input);

        return true;

    }

    /**
     * Beweeg naar Achteren
     * @param input
     * @return true als er naar achteren kon worden bewogen 
     */
    private void moveBackwards(BufferedReader input) {
        try {
            while (mapCalculator.getDistanceLeft(position) == defaultDistance && mapCalculator.getDistanceRight(position) == defaultDistance) {
                robot.sendCommand("P1.MOVEBW " + defaultDistance);
                result = input.readLine();
                updateRobotPosition(input);
            }
            robot.sendCommand("P1.MOVEBW " + defaultDistance);
            result = input.readLine();
            updateRobotPosition(input);


            if (turnRight(input)) {
            } else {
                turnLeft(input);
            }

        } catch (IOException ex) {
        }
        updateRobotPosition(input);

    }

    /**
     * update de actuele positie van de robot
     * opslag in position
     * tekent ook de afgelegde weg van de robot
     * @param input
     */
    private void updateRobotPosition(BufferedReader input) {
        try {
            robot.sendCommand("R1.GETPOS");
            result = input.readLine();
            parsePosition(result, position);
            drawRobotMovements(position);

        } catch (IOException e) {
        }
    }

    /**
     * update the occupancy map met resultaten scan
     * 
     * @param position
     * @param measures afstandsmetingen
     * @param sensorname 
     */
    private void updateOCMap(double[] position, double[] measures, String sensorName) {
        map.drawSensorScan(position, measures, robot.getSensorNameIndex(sensorName), robot.getSensorNameIndex(sensorName).getRange());
    }

    private void parsePosition(String value, double position[]) {
        int indexInit, indexEnd;
        String parameter;
        indexInit = value.indexOf("X=");
        parameter = value.substring(indexInit + 2);
        indexEnd = parameter.indexOf(' ');
        position[0] = Double.parseDouble(parameter.substring(0, indexEnd));

        indexInit = value.indexOf("Y=");
        parameter = value.substring(indexInit + 2);
        indexEnd = parameter.indexOf(' ');
        position[1] = Double.parseDouble(parameter.substring(0, indexEnd));

        indexInit = value.indexOf("DIR=");
        parameter = value.substring(indexInit + 4);
        position[2] = Double.parseDouble(parameter);

        position[0] = Math.round(position[0]);
        position[1] = Math.round(position[1]);

    }

    /**
     * Scan de huidige positie Robot/Platform
     * scant rondom, 360 metingen, 1 per graad
     * @param input
     * @param command scan commando
     * @param measures afstandsmetingen
     * @return measures afstanden tot obstacles in centimeters 
     */
    private double[] scanCurrentPosition(BufferedReader input, String command, double[] measures, String sensorName) {
        try {
            robot.sendCommand(command);
            result = input.readLine();

            // bepaal detectierange van de sensor
            double detectionRange = robot.getSensorNameIndex(sensorName).getRange();
            // verwerk metingen
            parseMeasures(result, measures, detectionRange);

        } catch (IOException e) {
        } finally {
            return measures;
        }
    }

    /**
     * parse de afstandsmetingen van de verschillende sensoren
     * @param value de datastring ontvangen van de sensoren
     * @param measures het tijdelijke array
     */
    private void parseMeasures(String value, double measures[], double detectionRange) {
        for (int i = 0; i < 360; i++) {
            measures[i] = detectionRange;   // was 100.0;
        }
        if (value.length() < 5) {
            return;
        }
        value = value.substring(5);  // removes the "SCAN " keyword
        StringTokenizer tokenizer = new StringTokenizer(value, " ");
        double dist;
        int dir;
        while (tokenizer.hasMoreTokens()) {
            dist = Double.parseDouble(tokenizer.nextToken().substring(2));
            dir = (int) Math.round(Math.toDegrees(Double.parseDouble(tokenizer.nextToken().substring(2))));
            if (dir == 360) {
                dir = 0;
            }
            measures[dir] = dist;
        }
    }

    /**
     * werk de OC map bij met actuele metingen
     * @param input
     */
    private void draw(BufferedReader input) {

        updateOCMap(position, scanCurrentPosition(input, "L1.SCAN", measures, "L1"), "L1");

        if (useSonar) {
            updateOCMap(position, scanCurrentPosition(input, "S1.SCAN", measuresSonar, "S1"), "S1");
            mergeMeasures(measures, measuresSonar);
        }
        // verbeterde empty velden verwerking
        map.drawEmpties(position, measures, null);
        // houdt het aantal scans bij
        draws++;
        System.out.println("----------------------------");
        System.out.println(draws);
        System.out.println("----------------------------");
    }

    /**
     * combineer de metingen van de Sonar met de Laser
     * zo voorkom je dat measures weer worden geinitieerd voorafgaand aan de sonar meting
     * let op: initieel staat measures op detectionRange (=100)
     * @param measures
     * @param measuresSonar 
     */
    private void mergeMeasures(double[] measures, double[] measuresSonar) {
        // get detectie range per sensor 
        double rangeL1 = robot.getSensorNameIndex("L1").getRange();
        double rangeS1 = robot.getSensorNameIndex("S1").getRange();

        for (int i = 0; i < 360; i++) {
            // is de meting van de sonar kleiner dan de laser (lasemeting-max scanbereik)
            // dan is er waarschijnlijk een opaque obstakel ontdekt
            if (!(measuresSonar[i] < 0) && measuresSonar[i] < rangeS1) {
                if (measures[i] > measuresSonar[i]) {
                    measures[i] = measuresSonar[i];
                }
            }
        }
    }

    /**
     * teken de afgelegde weg van de robot
     * slaat de afgelegde weg op in een overlay grid in de OC map
     * @param position 
     */
    private void drawRobotMovements(double[] position) {
        map.setGridOverlay((int) position[0], (int) position[1]);
        map.repaint();
    }

    /**
     * teken optioneel de afstandsmetingen
     */
    private void showMeasurements() {
        System.out.println("DIS voor gec. = " + mapCalculator.getDistanceFront(position) + " Dis voor " + mapCalculator.getFreeDistance(position, 0, 40));
        System.out.println("DIS rechts  = " + mapCalculator.getDistanceRight(position) + " Dis rechts " + mapCalculator.getFreeDistance(position, 90, 40));
        System.out.println("DIS links  = " + mapCalculator.getDistanceLeft(position) + " Dis links " + mapCalculator.getFreeDistance(position, 270, 40));
        System.out.println("DIS achter  = " + mapCalculator.getDistanceBack(position) + " Dis achter " + mapCalculator.getFreeDistance(position, 180, 40));
    }
}
